package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.CounterBase;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.Jaguar;
import edu.wpi.first.wpilibj.PIDController;


public  class Collar {

    private static final double 
        Kp = 0.9,
        Ki = 0.0,
        Kd = 0.15,

        collarDistancePerRev = 1.0/10.0,
        collarEncoderRevPerCount = 1.0/128.0,

        collarSetPointTolerance = 0.01,

        collarHomingSeqFwdSpeed = 0.6,
        collarHomingSeqRevSpeed = -0.6,

        collarHomePosition = 0.5;
    
    public static final double 
        collarStartPosition = 0.5,
        collarFender = 2.5,
        collarLow = 1.25,
        collarHigh = 1.0,
        collarKey = 0.75;
    
    
    private static double 
        mPositionSetpoint = 0,
        PWMOutput = 0;
    
    Jaguar collarPWM = new Jaguar(RobotMap.collarMotorPWM);
    DigitalInput collarFwdLimitSensor = new DigitalInput(RobotMap.collarFowardLimDin);
    DigitalInput collarRevLimitSensor = new DigitalInput(RobotMap.collarReverseLimDin);
    Encoder collarEncoder = new Encoder(RobotMap.CollarEncoderAChannelDin, RobotMap.CollarEncoderBChannelDin, false,CounterBase.EncodingType.k4X);
    PIDController collarPIDController = new PIDController(Kp, Ki, Kd, collarEncoder, collarPWM, 0.020);
    
    public Collar() {
        collarEncoder.setDistancePerPulse(collarDistancePerRev*collarEncoderRevPerCount);
        collarEncoder.setPIDSourceParameter(Encoder.PIDSourceParameter.kDistance);
        //NEED TO CHANGE LATER!!!
        collarPIDController.setInputRange(0.0, 2.7);
    }
    
    public void collarInit()
    {
        // Home Subsystem to initialize position encoder

	// If already on reverse limit, move forward off home position
	// to provide a repeatable homing sequence.
        while(collarRevLimitSensor.get() == false) {
            collarPWM.set(collarHomingSeqRevSpeed);
        }
        if(collarRevLimitSensor.get() == true) {
            // Move forward off home position until clear of limit
            while(collarRevLimitSensor.get() == true) {
                collarPWM.set(collarHomingSeqFwdSpeed);
            }
	// Start homing sequence by moving toward reverse limit
	// position and stop when reverse limit reached.
        }
        
        collarPWM.set(0.0);
        collarEncoder.reset();
        collarEncoder.start();
        
        // Enable the PID Controller to start PID running in the background
	// and set the starting setpoint 
        collarPIDController.enable();
        collarPIDController.setSetpoint(collarHomePosition);
    }
    
    public void setPosition(double positionSetpoint){
        mPositionSetpoint = positionSetpoint;
        collarPIDController.setSetpoint(positionSetpoint);
    }
    
    double getPosition() {
        return(collarEncoder.getDistance());
    }
    
    boolean atPosition() {
        return ( Math.abs(collarEncoder.getDistance() - mPositionSetpoint) < collarSetPointTolerance );
    }
    
    void PIDWrite(float PIDOutput) {
        // Check if reverse limit sensor is active when traveling
	// in the reverse direction and stop movement
        if ( (PIDOutput < 0.0) && (collarRevLimitSensor.get() == true) )
	{
		PWMOutput = 0.0;
	}
        
        // Check if forward limit sensor is active when traveling
	// in the forward direction and stop movement
	if ( (PIDOutput > 0.0) && (collarFwdLimitSensor.get() == true) )
	{
		PWMOutput = 0.0;
	}
        // Drive the Subsystem motor according to qualified PID output
	collarPWM.set(PWMOutput);
    }
    
    boolean atCollarFwdLimit() {
        if (collarFwdLimitSensor.get() == true) {
			return true;
		}
	else {
            return false;
        }
    }
    boolean atCollarRevLimit() {
        if(collarRevLimitSensor.get() == true) {
            return true;
        }
        else {
            return false;
        }
    }
}
